#include "PDController.h"
#include "KalmanTank.h"

PDController* PDController::_instance = 0;

PDController::PDController(void)
{
}


PDController::~PDController(void)
{
}

PDController* PDController::instance() {
	if (!_instance)
		_instance = new PDController();

	return _instance;
}

Command PDController::GetCommand(tank_t* tank, Vector2 & trajectory) {
	if (trajectory.x == 0 && trajectory.y == 0)
		return Command(tank->index, 0, 0, false);

	float tthreshold = M_PI / 4.0f;
	double tmag = 40;
	double speed = 0;

	double tmptheta =  trajectory.angle();
	double tmp2theta = normalizeAngle(tank->angle);

	double dtheta = trajectory.angle() - normalizeAngle(tank->angle);

	if(dtheta > M_PI)
		dtheta -= 2.0f*M_PI;
	else if(dtheta < -M_PI)
		dtheta += 2.0f*M_PI;

	Vector2 tank_cur_trajectory(cos(tank->angle), sin(tank->angle));
	double dot = Vector2::dot(trajectory, tank_cur_trajectory);

	double angvel = 0;
	if (dtheta > tthreshold) {
		speed = 1;
		angvel = 1;
	}
	else if (abs(dtheta) > tthreshold) {
		speed = 1;
		angvel = -1;
	}
	else {
		speed = 1.0 / (1.0 + 1.0*fabs(2 * dtheta));
		angvel = dtheta / tthreshold;
	}

	speed = 1.0 / (1.0 + 1.0*fabs(2 * dtheta));

	Vector2 tank_pos(tank->pos[0], tank->pos[1]);

	return Command(tank->index, speed, angvel, false);
}

Command PDController::GetWobblyCommand(tank_t* tank, Vector2 & trajectory) {
	Command cmd = GetCommand(tank, trajectory);
	if (cmd.angvel() < 0) {
		cmd.angvel(-1);
	}
	else {
		cmd.angvel(1);
	}
	cmd.speed(1);
	return cmd;
}

double PDController::normalizeAngle(double angle) {
	if (angle > 0) {
		while (angle > 2 * M_PI) {
			angle -= 2 * M_PI;
		}
	} else {
		while (angle < 0) {
			angle += 2 * M_PI;
		}
	}

	return angle;
}